#include <dispatch_server/deadlock_handler.h>
#include <vector>
#include <dispatch_server/dispatch_server.h>
#include <sch_common/sch_common.h>

namespace moying
{
namespace navigation
{

using namespace moying_proto;
using json = nlohmann::json;

//局部地图宽度(米)
#define TEMP_GOAL_AREA_WIDTH  20
//机器人死锁距离.超出该距离的机器人不认为是同一组
#define DEADLOCK_GROUP_MAX_DIS    5
#define CFG_DIR  "./configs/"

int DeadLockHandler::next_group_id=1;

DeadLockHandler::DeadLockHandler(MoyingMapPtr moying_map){
    moying_map_ = moying_map;
    init();
}

bool DeadLockHandler::init(){
    std::string avoid_points_path = std::string(CFG_DIR)+"conflict_areas.json";
    printf("%s\n",avoid_points_path.c_str());
    try{
        std::string file_data;
        if( !readFileData(avoid_points_path,file_data) )
            return false;
        auto js = json::parse(file_data);
        auto conflict_areas = js["conflict_areas"];
        for(auto area : conflict_areas){
            ConflictArea ca;
            ca.area_coor.set_x(area["conflict_area_coor"][0]);
            ca.area_coor.set_y(area["conflict_area_coor"][1]);
            ca.dis_tolerance = area["dis_tolerance"];
            ca.only_use_custom = area["only_use_custom"];
            for(auto pt : area["avoid_pts"]){
                moying_proto::Pose2D pose;
                pose.set_x(pt[0]);
                pose.set_y(pt[1]);
                pose.set_theta(pt[2]);
                ca.custom_avoid_pts.emplace_back(pose);
            }
            custom_conflict_area_.emplace_back(ca);
        }
    }catch(json::exception e){
        WARNING(e.what());
        WARNING("read custom avoid points failed!");
        return false;
    }
    HIGHLIGHT("read custom avoid points successfully!");
    return true;

}
    
void DeadLockHandler::handleDeadLock(){

    // HIGHLIGHT("begin deadlock handle...");
    //更新死锁机器人组
    groupDeadLockRobot();
    //计算机器人的临时导航点
    calculateTempGoal();

    auto isDeadLockGroupAllPathPlanSuccess = [](const DeadlockRobotGroup& group )->bool{
        for(auto info : group.deadlock_robots){
            if(!info.solved)
                return false;
        }
        return true;
    };
    
    auto clear_deadlock_group_of_robot = [](const DeadlockRobotGroup& group){
        for(auto info : group.deadlock_robots){
            info.robot->set_deadlock_group_id(0);
        }
    };

    std::lock_guard<std::recursive_mutex> lock(group_mutex_);
    for(auto iter=deadlock_robot_group_.begin();iter!=deadlock_robot_group_.end();){

        auto& group = *iter; 
        bool delete_group = false;
        bool wait = false;
        for(auto info : group.deadlock_robots){
            //检测是否有机器人在重新尝试全局路径规划或者在临时避让过程中
            // if(info.retry_global_path_plan || 
            //     info.path_plan_state == kWaitPathPlanResult 
            //     ){
            //     wait = true;
            //     break;
            // }else 
            if(info.path_plan_state == kWaitPathPlanResult || info.path_plan_state == kWaitNavArrive){
                wait = true;
                break;
            }
        }

        if(wait){
            if(group.begin_wait_time != 0 && time(0) > group.begin_wait_time+60){ //死锁组如果超时没处理,请求一遍全局路径规划后,删除死锁组
                retryGlobalPathPlan(group,0,true);
                WARNING("deadlock group wait timeout, delete, group,id:"<<group.group_id);
                clear_deadlock_group_of_robot(*iter);
                iter = deadlock_robot_group_.erase(iter);
            }else{
                iter++;
                retryGlobalPathPlan(group,3);
            }
            continue;
        }

        if(isDeadLockGroupAllPathPlanSuccess(group)){
            delete_group = true;
        }else{
            if(!selectRobotToAvoid(group)){
                delete_group = true;
                retryGlobalPathPlan(group,0,true);
            }else{
                // retryGlobalPathPlan(group,2);
            }
        }

        if(delete_group){
            WARNING("delete deadlock group,id:"<<group.group_id);
            clear_deadlock_group_of_robot(*iter);
            iter = deadlock_robot_group_.erase(iter);
        }else{
            iter++;
        }
    }

    // HIGHLIGHT("end deadlock handle");
}

bool DeadLockHandler::removeRobotFromDeadlockGroup(unsigned int robot_id){
    std::lock_guard<std::recursive_mutex> lock(group_mutex_);
    for(auto iter=deadlock_robot_group_.begin(); iter != deadlock_robot_group_.end(); iter++){
        bool in_group = false;
        for(auto iter2=iter->deadlock_robots.begin();iter2 != iter->deadlock_robots.end();iter2++){
            if(iter2->robot->getID() == robot_id){
                iter->deadlock_robots.erase(iter2);
                iter2->robot->set_deadlock_group_id(0);
                in_group = true;
                break;
            }
        }
        if(in_group && iter->deadlock_robots.empty()){
            deadlock_robot_group_.erase(iter);
            break;
        }
    }
    return true;
}

void DeadLockHandler::groupDeadLockRobot(){

    if(moying_map_ == nullptr)
        return;

    std::vector< std::pair<VehiclePtr,DeadlockRobotGroup*> > path_plan_failed_robots; 

    //检测机器人是否已经在死锁组里
    auto checkVehicleInDeadLockGroup = [this](VehiclePtr vehicle)->bool{
        std::lock_guard<std::recursive_mutex> lock(group_mutex_);
        for(auto group : deadlock_robot_group_){
            for(auto info : group.deadlock_robots){
                if(vehicle == info.robot){
                    return true;
                }
            }
        }
        return false;
    };

    //尝试把机器人添加到已有死锁组里，并返回添加结果
    auto addRobotToDeadLockGroup = [this](VehiclePtr vehicle)->bool{
        auto rs = vehicle->getPose();
        std::lock_guard<std::recursive_mutex> lock(group_mutex_);
        for(auto& group : deadlock_robot_group_){
            for(auto info : group.deadlock_robots){
                VehicleState state = info.robot->getPose();
                float x_delta = fabs(rs.x-state.x);
                float y_delta = fabs(rs.y-state.y);
                float dis = sqrt(x_delta*x_delta+y_delta*y_delta);
                if(dis < DEADLOCK_GROUP_MAX_DIS){
                    DeadLockRobotHandleInfo info;
                    info.robot = vehicle;
                    group.deadlock_robots.push_back(info);
                    vehicle->set_deadlock_group_id(group.group_id);
                    WARNING("add robot to exist group,group id:"<<group.group_id<<" robot_id:"<<vehicle->getID());
                    return true;
                }
            }
        }
        return false;
    };

    //找出未分组的死锁机器人
    moying_map_->ergodicRobots([=,this,&path_plan_failed_robots](VehiclePtr vehicle)->bool{

        if(vehicle == nullptr)
            return true;
        
        if(  vehicle->last_path_plan_result() == 1 || vehicle->isRobotInEmergencyObstacle() || vehicle->path_plan_fail_times() < 2 ){
          
            //  printf("====== vehicle->last_path_plan_result:%d robot_id:%d fail_times:%d\n",
            //     vehicle->last_path_plan_result(),vehicle->getID(),vehicle->path_plan_fail_times());
            return true;
         }

        if( checkVehicleInDeadLockGroup(vehicle) )
            return true;

        if(!addRobotToDeadLockGroup(vehicle)){
            path_plan_failed_robots.push_back(std::make_pair(vehicle,nullptr));
        }

        return true;
    });

// printf(">>>>>> path_plan_failed_robots.size() : %d\n",path_plan_failed_robots.size());

    //对新的规划失败的机器人分组
    for(int i=0;i<path_plan_failed_robots.size();i++){//for1

        VehiclePtr ri = path_plan_failed_robots[i].first;

        for(int j=i+1;j<path_plan_failed_robots.size();j++){//for2
            VehiclePtr rj = path_plan_failed_robots[j].first;
            auto state1 = ri->getPose();
            auto state2 = rj->getPose();
            float x_delta = fabs(state1.x-state2.x);
            float y_delta = fabs(state1.y-state2.y);
            float dis = sqrt(x_delta*x_delta+y_delta*y_delta);
            if(dis < DEADLOCK_GROUP_MAX_DIS){
                
                if(path_plan_failed_robots[i].second != nullptr && path_plan_failed_robots[j].second != nullptr )
                    continue;

                DeadlockRobotGroup *p_group=path_plan_failed_robots[i].second;
                if(p_group == nullptr){

                    DeadLockRobotHandleInfo info;
                    if(path_plan_failed_robots[j].second == nullptr){
                        std::lock_guard<std::recursive_mutex> lock(group_mutex_);
                        DeadlockRobotGroup group;
                        group.group_id = next_group_id++;
                        info.robot = ri;
                        group.deadlock_robots.push_back(info);
                        info.robot = rj;
                        group.deadlock_robots.push_back(info);
                        group.begin_wait_time = 0;
                        deadlock_robot_group_.push_back(group);
                        group.custom_conflict_area = nullptr;
                        p_group = &deadlock_robot_group_.back();
                        path_plan_failed_robots[i].second = p_group;
                        path_plan_failed_robots[j].second = p_group;
                        WARNING("create deadlock group,id:"<<group.group_id<<" robot1:"<<ri->getID()<<" robot2:"<<rj->getID());

                    }else{
                        p_group=path_plan_failed_robots[j].second;
                        info.robot = ri;
                        p_group->deadlock_robots.push_back(info);
                        path_plan_failed_robots[i].second = p_group;
                    }
                }else{
                    DeadLockRobotHandleInfo info;
                    info.robot = rj;
                    p_group->deadlock_robots.push_back(info);
                    path_plan_failed_robots[j].second = p_group;
                }

                ri->set_deadlock_group_id(p_group->group_id);
                rj->set_deadlock_group_id(p_group->group_id);
            }
        }//for2
    }//for1
}

bool DeadLockHandler::isRobotInDeadLockGroup(VehiclePtr vehicle){
    std::lock_guard<std::recursive_mutex> lock(group_mutex_);
    for(auto group : deadlock_robot_group_){
        for(auto info : group.deadlock_robots){
            if(info.robot == vehicle)
                return true;
        }
    }
    return false;
}


void  DeadLockHandler::updatePathPlanResult(VehiclePtr vehicle,bool result,VehicleState goal){
    std::lock_guard<std::recursive_mutex> lock(group_mutex_);
    for(auto& group : deadlock_robot_group_){//for1
    
        for(auto iter=group.deadlock_robots.begin();iter!=group.deadlock_robots.end();iter++){//for2
            auto& info = *iter;
            if(info.robot == vehicle){//if1
                if(info.retry_global_path_plan){
                    info.retry_global_path_plan = false;
                    info.solved = result;
                    HIGHLIGHT("global path plan result:"<<(result?1:0)<<" robot_id:"<<vehicle->getID());
                    if(result){
                        group.deadlock_robots.erase(iter);  //全局路径规划成功,从死锁组删除
                        vehicle->set_deadlock_group_id(0);
                        return;
                    }else{
                        bool temp_avoiding = false;
                        for(auto info : group.deadlock_robots){
                            //检测是否有机器人在重新尝试全局路径规划或者在临时避让过程中
                            if( info.path_plan_state == kWaitNavArrive){
                                temp_avoiding = true;
                                break;
                            }
                        }
                        if(temp_avoiding){//如果有机器人在局部避让中,可以让其他机器人2s后尝试全局路径规划
                            retryGlobalPathPlan(group,2);
                        }
                    }
                }else{
                    int pos_index = info.next_pos_index-1;
                    if(pos_index < 0 || pos_index >= info.temp_goals.size()){
                        return;
                    }
                    auto cur_goal = info.temp_goals[pos_index];
                    if( SAME_POSE(cur_goal,goal) ){
                        if(result){
                            info.path_plan_state = kWaitNavArrive;
                            info.avoid_handled = true;
                        }else{
                            info.path_plan_state = kPathPlanInit;
                            info.avoid_handled  = false;
                        }
                        HIGHLIGHT("temp avoid path plan result:"<<(result?1:0)<<" robot_id:"<<vehicle->getID());
                    }
                }
                return;
            }//if1
        }//for2
    }//for1
}

bool DeadLockHandler::handleNavStateNotify(moying_proto::NavStateNotify notify){

    NavStateInfo state_info = (NavStateInfo)notify.state_info();
    IdType robot_id = notify.robot_id();

    std::lock_guard<std::recursive_mutex> lock(group_mutex_);
    for(auto& group : deadlock_robot_group_){//for1
        for(auto& info : group.deadlock_robots){//for2
            if(info.robot->getID() == robot_id){//if1
                if(info.next_pos_index <= 0 || info.next_pos_index > info.temp_goals.size() || info.path_plan_state != kWaitNavArrive){
                    printf(" !!!! DeadLockHandler::handleNavStateNotify return false, info.path_plan_state:%d robot_id:%d\n",
                        info.path_plan_state,robot_id);
                    return false;
                }
                auto temp_goal = info.temp_goals[info.next_pos_index-1];
                auto nav_goal = state_info.pose();

                // //导航的目标点必须是临时避让目标点才处理
                if(fabs(temp_goal.x-nav_goal.x())+fabs(temp_goal.y-nav_goal.y()) <= SAME_POINT_COOR_DIS 
                    && fabs(temp_goal.th-nav_goal.theta()) < SAME_POINT_THETA_DIS)
                {
                    // if(state_info.state() != NavState::kArriveFinalDestPos && state_info.state() != NavState::kNavFailed){
                    //     return false;
                    // }
                    if(state_info.state() == NavState::kArriveStepDestPos){
                        return false;
                    }

                    if(state_info.state() == NavState::kArriveFinalDestPos){
                        HIGHLIGHT("nav to temp goal successfully,robot_id:"<<info.robot->getID());
                        info.avoid_handled = true;
                        info.path_plan_state = kPathPlanInit;
                    }
                    if(state_info.state() == NavState::kNavFailed){
                        WARNING("nav to temp goal failed,robot_id:"<<info.robot->getID());
                        info.avoid_handled = true;
                        info.path_plan_state = kPathPlanInit;        
                    }
                    HIGHLIGHT("nav to temp goal result:"<<state_info.state() <<" ,robot_id:"<<info.robot->getID());
                    
                    return true;
                }
                
            }//if1
        }//for2
    }//for1
    return true;
}

void DeadLockHandler::handleRobotStopNav(unsigned int robot_id){

    // std::lock_guard<std::recursive_mutex> lock(group_mutex_);
    // for(auto& group : deadlock_robot_group_){//for1
    //     for(auto& info : group.deadlock_robots){//for2
    //         if(info.robot->getID() == robot_id){//if1
    //             if(info.path_plan_state != kWaitNavArrive){
    //                 return;
    //             }

    //             info.avoid_handled = true;
    //             retryGlobalPathPlan(group,0);
    //             return;
                
    //         }//if1
    //     }//for2
    // }//for1
}

int DeadLockHandler::retryGlobalPathPlan(DeadlockRobotGroup& group,int path_plan_delay_seconds,bool request_all){

    int count=0;
    for(auto& dinfo : group.deadlock_robots ){
        if(request_all){
            if(dinfo.solved)
                continue;
        }else if(dinfo.solved || dinfo.retry_global_path_plan || dinfo.path_plan_state == kWaitPathPlanResult || dinfo.path_plan_state == kWaitNavArrive){
            continue;
        }

        count++;
        dinfo.retry_global_path_plan = true;
        PathPlanningRequest request;
        VehicleState start,goal;
        start = dinfo.robot->getPose();
        goal = dinfo.robot->global_goal();
        request.vehicle = dinfo.robot;
        request.start   = start;
        request.goal    = goal;
        request.is_global_goal = true;
        request.path_plan_start_time = path_plan_delay_seconds > 0 ? time(0)+path_plan_delay_seconds : 0;
        request.occupy_time_after_arrive = dinfo.robot->occupy_time_after_arrive();
        DispatchServer::instance()->addPathPlanRequest(dinfo.robot->getID(),request);
        HIGHLIGHT( "retry global goal path plan after temp avoid,goal_x:"
                <<goal.x<<",goal_y:"<<goal.y<<",theta:"<<goal.th<<",robot_id:"<<dinfo.robot->getID() );

    }
    // if(count > 0){
    //     group.begin_wait_time = time(0);
    // }
    return count;
}

bool DeadLockHandler::selectRobotToAvoid(DeadlockRobotGroup& group){

    bool select = false;
    int max_weight_robot_index = 0;
    int max_weight = INT_MIN;
    for(int i=0;i<group.deadlock_robots.size();i++){
        auto& info = group.deadlock_robots[i];
        if(info.solved || info.avoid_handled || info.retry_global_path_plan)   //已经成功进行了全局路径规划,或者进行过避障处理,忽略
            continue;
        if(info.next_pos_index >= info.temp_goals.size() || info.next_pos_index < 0){
            info.avoid_handled = true;
            HIGHLIGHT("set avoid handled flag to true,robot_id:"<<info.robot->getID());
            continue;
        }
        if(info.path_plan_state != kPathPlanInit)
            continue;

        bool has_valid_goal = false;
        for(int n=info.next_pos_index;n<info.temp_goals.size();n++){
            auto goal = info.temp_goals[n];
            if( moying_map_->isVehicleFreeLocateCellInRecentSeconds(info.robot->getID(),goal.x,goal.y,20) ){
                info.next_pos_index = n;
                has_valid_goal = true;
                break;
            }else{
                continue;
            }
        }
        if(!has_valid_goal)
            continue;

        if(info.robot_weight > max_weight){
            max_weight_robot_index = i;
            max_weight = info.robot_weight;
            select = true;
        }
    }

    if(!select)
        return false;

    //尝试规划到下一个临时目标点
    auto& info = group.deadlock_robots[max_weight_robot_index];
    PathPlanningRequest request;
    VehicleState start,goal;
    start = info.robot->getPose();
    goal = info.temp_goals[info.next_pos_index++];
    request.vehicle = info.robot;
    request.start   = start;
    request.goal    = goal;
    request.is_global_goal      = false;

    //临时避让时限制搜索范围,降低搜索时间
    request.search_area.valid   = true;
    request.search_area.min_cell_x = info.avoid_area_minx;
    request.search_area.min_cell_y = info.avoid_area_miny;
    request.search_area.max_cell_x = info.avoid_area_maxx;
    request.search_area.max_cell_y = info.avoid_area_maxy;

    DispatchServer::instance()->addPathPlanRequest(info.robot->getID(),request);
    info.path_plan_state = kWaitPathPlanResult;

    int mx,my;
    moying_map_->getCostMap()->worldToMap(goal.x,goal.y,mx,my);
    HIGHLIGHT("try temp avoid goal path plan ,goal_x:"<<mx<<" goal_y:"<<my<<" goal_theta:"<<goal.th<<" robot_id:"<<info.robot->getID());
    group.begin_wait_time = time(0);
    return true;
}


void DeadLockHandler::calculateTempGoal(){

    if(moying_map_ == nullptr)
        return;

    //计算机器人周围临时目标点的权重，group是机器人所属的组，info是要计算的机器人信息
    auto calcuateTempGoalWeight = [this](const DeadlockRobotGroup& group,DeadLockRobotHandleInfo& info){
        //计算局部地图cell边界
        VehicleState state = info.robot->getPose();
        float res = moying_map_->getCostMap()->getResolution();
        int center_x,center_y;
        moying_map_->getCostMap()->worldToMap(state.x,state.y,center_x,center_y);
        int local_width = ceil(TEMP_GOAL_AREA_WIDTH / res);
        int minx,miny,maxx,maxy;
        minx = center_x-local_width/2;
        miny = center_y-local_width/2;
        minx = minx<0?0:minx;
        miny = miny<0?0:miny;
        maxx = minx + local_width;
        maxy = miny + local_width;
        int cellx = moying_map_->getCostMap()->getSizeInCellsX();
        int celly = moying_map_->getCostMap()->getSizeInCellsY();
        maxx = maxx >=  cellx ? cellx-1 : maxx; 
        maxy = maxy >=  celly ? celly-1 : maxy; 

        info.avoid_area_minx = minx;
        info.avoid_area_miny = miny;
        info.avoid_area_maxx = maxx;
        info.avoid_area_maxy = maxy;

        float length = info.robot->getLength();
        float width = info.robot->getWidth();

        //计算和机器人距离超过一米的点的权重,按权重大小排序，最多选择权重最大的三个导航点
        std::multimap<int,VehicleState> temp_goal;
        int min_dis=INT32_MAX;  //距离障碍物的最小距离 
        for(int x=minx;x<=maxx;x++){
            for(int y=miny;y<=maxy;y++){
                int dis = abs(center_x-x)+abs(center_y-y);
                unsigned char cost = moying_map_->getCostMap()->getCost(x,y);
                if(cost == MAPCOSTS::OBSTACLE  || cost == MAPCOSTS::INFLATED_OBSTACLE ){
                    min_dis = min_dis <= dis ? min_dis : dis;
                    continue;
                }
                
                auto correct_cells = moying_map_->corrected_cells(info.robot);
                if(correct_cells == nullptr)
                    continue;
                auto cci = (*correct_cells)[x][y];

                if( cci.cell_x == -1)    //该点不可通行
                    continue;
                if(dis*res >= 0.3 ){ //临时导航点必须和原来坐标有一定距离，否则没意义

                    //临时目标点和死锁组中所有机器人的距离的和,作为目标点的选择优先级权重
                    int goal_weight=0;
                    for(auto info:group.deadlock_robots){
                        int rmx,rmy;
                        auto r = info.robot->getPose();
                        moying_map_->getCostMap()->worldToMap(r.x,r.y,rmx,rmy);
                        int w = abs(rmx-x)+abs(rmy-y);
                        goal_weight += w;
                        // goal_weight -= cci.cost;
                        if(moying_map_->getCostMap()->getCost(x,y) != MAPCOSTS::FREE){
                            goal_weight -= 10;
                        }

                        auto all_owr = moying_map_->all_one_way_road();
                        for(auto owr : *all_owr){
                            if(owr->isCellInRoad(MapCell(x,y))){
                                goal_weight -= 50;
                                break;
                            }
                        }
                    }

                    VehicleState state;
                    if( !moying_map_->getCellNavCoor(x,y,state.x,state.y,info.robot) )
                        continue;

                    std::vector<double> thetas;
                    auto theta = (*correct_cells)[x][y].theta;
                    for(int i=0;i<8;i++){
                        if(1<<i & theta){
                            thetas.emplace_back(info.robot->getTheta(i));
                            // state.th = info.robot->getTheta(i);
                            // break;
                        }
                    }
                    srand(time(0));
                    if( thetas.empty() )
                        continue;
                    int index = (int)( (double)rand()/(double)RAND_MAX * thetas.size() )  %  thetas.size();
                    state.th = thetas[index];

                    if( moying_map_->checkCollision(length+0.4,width+0.4,state.x,state.y,state.th,info.robot) ){
                        continue;
                    }
                    temp_goal.emplace(goal_weight,state);
                }
            }
        }

        // printf("====print temp goal\n");
        for(auto item : temp_goal){
            info.temp_goals.emplace_back(item.second);
            // int mx,my;
            // moying_map_->getCostMap()->worldToMap(goal.second.x,goal.second.y,mx,my);
            // printf("robot %d temp goal mx:%d my:%d theta:%.2f\n",info.robot->getID(),mx,my,info.robot->getPose().th);
        }
        std::reverse(info.temp_goals.begin(),info.temp_goals.end());
        while(info.temp_goals.size() > 5){
            info.temp_goals.pop_back();
        }
        info.robot_weight = min_dis;
        HIGHLIGHT("generate "<<info.temp_goals.size()<<" temp goals,robot_id:"<<info.robot->getID());
 
    };

    std::lock_guard<std::recursive_mutex> lock(group_mutex_);
    for(auto& group : deadlock_robot_group_){
        if( !group.deadlock_robots[0].temp_goals.empty() ){
            continue;
        }

        //找出是否有自定义的冲突区避让点
        ConflictArea* ca = nullptr;
        for(auto& info : group.deadlock_robots){
            VehicleState vs;
            info.robot->getPose(vs);
            for(auto cca : custom_conflict_area_){
                float x_delta = vs.x-cca.area_coor.x();
                float y_delta = vs.y-cca.area_coor.y();
                float dis = sqrt(x_delta*x_delta+y_delta*y_delta);
                if(dis <= cca.dis_tolerance){
                    ca = &cca;
                    break;
                }
            }
            if(ca != nullptr)
                break;
        }

        std::vector<VehicleState> vss;
        if(ca != nullptr){

            for(auto pose : ca->custom_avoid_pts){
                VehicleState vs;
                vs.x = pose.x();
                vs.y = pose.y();
                vs.th = pose.theta();
                vss.emplace_back(vs);
            }

            //如果只使用自定义的避让点,不再计算其他避让点
            if( ca->only_use_custom ){

                float res = moying_map_->getCostMap()->getResolution();
                int local_width = ceil(ca->dis_tolerance*6 / res);
                int minx,miny,maxx,maxy;
                minx = ca->area_coor.x()-local_width/2;
                miny = ca->area_coor.y()-local_width/2;
                minx = minx<0?0:minx;
                miny = miny<0?0:miny;
                maxx = minx + local_width;
                maxy = miny + local_width;
                int cellx = moying_map_->getCostMap()->getSizeInCellsX();
                int celly = moying_map_->getCostMap()->getSizeInCellsY();
                maxx = maxx >=  cellx ? cellx-1 : maxx; 
                maxy = maxy >=  celly ? celly-1 : maxy; 

        

                for(auto& info : group.deadlock_robots){
                    if( !info.temp_goals.empty() ){    //已经计算过，不再重复计算
                        continue;
                    }
                    info.temp_goals = vss;

                    info.avoid_area_minx = minx;
                    info.avoid_area_miny = miny;
                    info.avoid_area_maxx = maxx;
                    info.avoid_area_maxy = maxy;
                }
                continue;
            }
        }

        for(auto& info : group.deadlock_robots){
            if( !info.temp_goals.empty() ){    //已经计算过，不再重复计算
                continue;
            }
            calcuateTempGoalWeight(group,info);
            if( !vss.empty() ){
                for(auto goal : info.temp_goals){
                    vss.emplace_back(goal);
                }
                info.temp_goals = vss;
            } 
        }
    }

}


}
}
